#include "ecat_manager.h"

#include "ethercattype.h"
#include "nicdrv.h"
#include "ethercatbase.h"
#include "ethercatmain.h"
#include "ethercatconfig.h"
#include "ethercatcoe.h"
#include "ethercatdc.h"
#include "ethercatprint.h"

#include "osal.h"
#include "osal_defs.h"

#ifndef EC_VER1
#define EC_VER1
#endif

#define ECAT_READWRITE_LOCK // 是否在EcatManager内启用读写锁
#define ECATMANAGER_USE_QREADWRITELOCK // 使用Qt提供的读写锁


#ifdef ECAT_READWRITE_LOCK
#   ifndef ECATMANAGER_USE_QREADWRITELOCK
#       define ECAT_STAT_LOCKER QMutexLocker locker(&stat_lock_);
#       define ECAT_CTRL_LOCKER QMutexLocker locker(&ctrl_lock_);
#   else // ECATMANAGER_USE_QREADWRITELOCK
#       define ECAT_STAT_LOCKER QReadLocker  rlocker(&rd_wr_lock_);
#       define ECAT_CTRL_LOCKER QWriteLocker wlocker(&rd_wr_lock_);
#   endif // ECATMANAGER_USE_QREADWRITELOCK
#else  // ECAT_READWRITE_LOCK
#   define ECAT_CTRL_LOCKER {}
#   define ECAT_STAT_LOCKER {}
#endif  // ECAT_READWRITE_LOCK

EcatManager::EcatManager(QObject *parent) : QObject(parent),
    ctrl_lock_(QMutex::RecursionMode::Recursive),
    stat_lock_(QMutex::RecursionMode::Recursive),
    rd_wr_lock_(QReadWriteLock::RecursionMode::Recursive)// 可重入的锁
{

}

EcatManager::~EcatManager()
{
    // static singleton instance destruction will never be
    // called before main() returns.
}

EcatManager* EcatManager::get_instance()
{
    static EcatManager instance;
    return &instance;
}

bool EcatManager::init_slaves(const QString &ifname)
{
    ifname_ = ifname;
    if (ec_init( const_cast<char*>(ifname.toStdString().c_str()) ))
    {
        qDebug() << QString("ec_init on %0 succeeded.").arg(ifname);

        // 寻找并配置从站
        if (ec_config(FALSE, &io_map_) > 0)
        {
            // 配置DC时钟
            ec_configdc();

            // 检测到的从站数量是否与定义的一致
            qDebug() << QString("%0 slaves found and configured.").arg(ec_slavecount);
            if (ec_slavecount != SLAVE_NUM)
            {
                fprintf(stderr, "defined slave num[%d] & ec_slavecount[%d] not matched\n", SLAVE_NUM, ec_slavecount);
                ec_close();
                return false;
            }

            // 计算expected_wkc
            expected_wkc_ = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
            qDebug() << QString("Calculated workcounter %0.").arg(expected_wkc_);
            if (expected_wkc_ != SLAVE_NUM * 3) {
                // 判断wkc是否正确
                ec_close();
                return false;
            }

            // 等待从站到达safe op模式
            if (!wait_slaves_to_safe_op_()) {
                ec_close();
                return false;
            }

            // 配置从站到达operational模式
            if (!conf_slaves_to_operational_()) {
                ec_close();
                return false;
            }


            // 配置PDO映射到mc_ctrl_和mc_stat_
            conf_servo_pdo_map_();
            conf_servo_default_op_();

            // 进行一次发送和接受
            ec_send_processdata();

            wkc_ = ec_receive_processdata(EC_TIMEOUTRET);
            if (wkc_ < expected_wkc_) {
                qDebug() << "wkc not matched";
                ec_close();
                return false;
            }

            // read之后获取当前位置，初始化驱动器的目标位置
            // 这一部分代码也可以在类外部做
            for (int i = 1; i <= SERVO_NUM; ++i) {
                position_actual_value_t act_pos = servo_state_actual_pos(i);
                qDebug() << QString("servo %0 act pos is : %1").arg(i).arg(act_pos);

                init_servo_position_(i); // 调用无锁的位置初始化私有函数
            }

            ec_send_processdata();

            return true;
        }
        else
        {
            qDebug() << QString("No slaves found!\n");
            return false;
        }

    }
    else
    {
        qDebug() << QString("No socket connection on %0 \nExcecute as root").arg(ifname);
        return false;
    }
}

bool EcatManager::try_init_slaves(const QString &ifname, int retry_times)
{
    Q_ASSERT(retry_times >= 0);
    int trying_times = 0;
    while (!init_slaves(ifname)) {
        ++trying_times;
        if (trying_times > retry_times) {
            return false;
        }
        qDebug() << "Retrying connect ethercat times: " << trying_times;
    }
    return true;
}

EcatManager::servo_stat_t EcatManager::get_servo_state(int slave_num)
{
    ECAT_STAT_LOCKER;
    ASSERT_SERVO_NUM(slave_num);
    return *servo_stat_[slave_num];
}

void EcatManager::get_servo_state(int slave_num, EcatManager::servo_stat_t &state_ref)
{
    ECAT_STAT_LOCKER;
    ASSERT_SERVO_NUM(slave_num);
    state_ref = *servo_stat_[slave_num];
    // memcpy(&state_ref, servo_stat_[slave_num], sizeof (EcatManager::servo_stat_));
}

bool EcatManager::servo_state_fault(int slave_num)
{
    ECAT_STAT_LOCKER;
    ASSERT_SERVO_NUM(slave_num);
    return is_SW_Fault(servo_stat_[slave_num]->status_word);
}

bool EcatManager::servo_state_switch_on_diabled(int slave_num)
{
    ECAT_STAT_LOCKER;
    ASSERT_SERVO_NUM(slave_num);
    return is_SW_SwitchOnDisabled(servo_stat_[slave_num]->status_word);
}

bool EcatManager::servo_state_ready_to_switch_on(int slave_num)
{
    ECAT_STAT_LOCKER;
    ASSERT_SERVO_NUM(slave_num);
    return is_SW_ReadyToSwitchOn(servo_stat_[slave_num]->status_word);
}

bool EcatManager::servo_state_switched_on(int slave_num)
{
    ECAT_STAT_LOCKER;
    ASSERT_SERVO_NUM(slave_num);
    return is_SW_SwitchedOn(servo_stat_[slave_num]->status_word);
}

bool EcatManager::servo_state_operation_enabled(int slave_num)
{
    ECAT_STAT_LOCKER;
    ASSERT_SERVO_NUM(slave_num);
    return is_SW_OperationEnabled(servo_stat_[slave_num]->status_word);
}

position_actual_value_t EcatManager::servo_state_actual_pos(int slave_num)
{
    ECAT_STAT_LOCKER;
    ASSERT_SERVO_NUM(slave_num);
    return servo_stat_[slave_num]->position_actual_value;
}

bool EcatManager::servo_state_has_fault()
{
    ECAT_STAT_LOCKER;
    for (int i = 1; i <= SERVO_NUM; ++i) {
        if (is_SW_Fault(servo_stat_[i]->status_word)) {
            return true;
        }
    }
    return false;
}

bool EcatManager::servo_state_all_operation_enabled()
{
    ECAT_STAT_LOCKER;
    for (int i = 1; i <= SERVO_NUM; ++i) {
        if (!is_SW_OperationEnabled(servo_stat_[i]->status_word)) {
            return false;
        }
    }
    return true;
}

void EcatManager::set_servo_ctrl(int slave_num, const EcatManager::servo_ctrl_t &ctrl_ref)
{
    ECAT_CTRL_LOCKER;
    ASSERT_SERVO_NUM(slave_num);
    *servo_ctrl_[slave_num] = ctrl_ref;
    // memcpy(servo_ctrl_[slave_num], &ctrl_ref, sizeof (EcatManager::servo_ctrl_));
}

void EcatManager::set_servo_control_word(int slave_num, control_word_t control_word)
{
    ECAT_CTRL_LOCKER;
    if (slave_num == 0) {
        for (int i = 1; i <= SERVO_NUM; ++i) {
            servo_ctrl_[i]->control_word = control_word;
        }
    } else {
        ASSERT_SERVO_NUM(slave_num);
        servo_ctrl_[slave_num]->control_word = control_word;
    }
}

void EcatManager::set_servo_operation_mode(int slave_num, operation_mode_t op_mode)
{
    ECAT_CTRL_LOCKER;
    if (slave_num == 0) {
        for (int i = 1; i <= SERVO_NUM; ++i) {
            servo_ctrl_[i]->modes_of_operation = op_mode;
        }
    } else {
        ASSERT_SERVO_NUM(slave_num);
        servo_ctrl_[slave_num]->modes_of_operation = op_mode;
    }
}

void EcatManager::set_servo_target_position(int slave_num, target_position_t target_pos)
{
    ECAT_CTRL_LOCKER;
    // 不提供slave_num = 0，一次性设置全部坐标的方式
    ASSERT_SERVO_NUM(slave_num);
    servo_ctrl_[slave_num]->target_position = target_pos;
}

void EcatManager::servo_set_shutdown(int slave_num)
{
    ECAT_CTRL_LOCKER;
    if (slave_num == 0) {
        for (int i = 1; i <= SERVO_NUM; ++i) {
            servo_ctrl_[i]->control_word = CW_Shutdown;
        }
    } else {
        ASSERT_SERVO_NUM(slave_num);
        servo_ctrl_[slave_num]->control_word = CW_Shutdown;
    }
}

void EcatManager::servo_set_switch_on(int slave_num)
{
    ECAT_CTRL_LOCKER;
    if (slave_num == 0) {
        for (int i = 1; i <= SERVO_NUM; ++i) {
            servo_ctrl_[i]->control_word = CW_SwitchOn;
        }
    } else {
        ASSERT_SERVO_NUM(slave_num);
        servo_ctrl_[slave_num]->control_word = CW_SwitchOn;
    }
}

void EcatManager::servo_set_enable_operation(int slave_num)
{
    ECAT_CTRL_LOCKER;
    if (slave_num == 0) {
        for (int i = 1; i <= SERVO_NUM; ++i) {
            servo_ctrl_[i]->control_word = CW_EnableOperation;
        }
    } else {
        ASSERT_SERVO_NUM(slave_num);
        servo_ctrl_[slave_num]->control_word = CW_EnableOperation;
    }
}

void EcatManager::servo_set_disable_operation(int slave_num)
{
    ECAT_CTRL_LOCKER;
    if (slave_num == 0) {
        for (int i = 1; i <= SERVO_NUM; ++i) {
            servo_ctrl_[i]->control_word = CW_DisableOperation;
        }
    } else {
        ASSERT_SERVO_NUM(slave_num);
        servo_ctrl_[slave_num]->control_word = CW_DisableOperation;
    }
}

void EcatManager::servo_set_fault_reset(int slave_num)
{
    ECAT_CTRL_LOCKER;
    // 清错时，将target_pos置为act_pos
    if (slave_num == 0) {
        for (int i = 1; i <= SERVO_NUM; ++i) {
            init_servo_position_(i);
            servo_ctrl_[i]->control_word = CW_FaultReset;
        }
    } else {
        ASSERT_SERVO_NUM(slave_num);
//        servo_ctrl_[slave_num]->target_position = servo_stat_[slave_num]->position_actual_value;
        init_servo_position_(slave_num);
        servo_ctrl_[slave_num]->control_word = CW_FaultReset;
    }
}

void EcatManager::servo_init_position(int slave_num)
{
    ECAT_CTRL_LOCKER;
    if (slave_num == 0) {
        for (int i = 1; i <= SERVO_NUM; ++i) {
            init_servo_position_(i);
        }
    } else {
        ASSERT_SERVO_NUM(slave_num);
        init_servo_position_(slave_num);
    }
}

void EcatManager::sync()
{
#ifdef ECAT_READWRITE_LOCK
#   ifndef ECATMANAGER_USE_QREADWRITELOCK
    ctrl_lock_.lock();
    stat_lock_.lock();
#   else // ECATMANAGER_USE_QREADWRITELOCK
    ECAT_CTRL_LOCKER;
#   endif // ECATMANAGER_USE_QREADWRITELOCK
#endif // ECAT_READWRITE_LOCK

    ec_send_processdata();
    wkc_ = ec_receive_processdata(EC_TIMEOUTRET);

    /* FOR DEBUG START*/
    if (wkc_ != expected_wkc_) {
        fprintf(stderr, "wkc[%d] != expected_wkc[%d]\n", wkc_, expected_wkc_);
    }
    /* FOR DEBUG END  */

#ifdef ECAT_READWRITE_LOCK
#   ifndef ECATMANAGER_USE_QREADWRITELOCK
    stat_lock_.unlock();
    ctrl_lock_.unlock();
#   endif
#endif // ECAT_READWRITE_LOCK
}

void EcatManager::read()
{
    ECAT_STAT_LOCKER;
    wkc_ = ec_receive_processdata(EC_TIMEOUTRET);

    /* FOR DEBUG START*/
    if (wkc_ != expected_wkc_) {
        fprintf(stderr, "wkc[%d] != expected_wkc[%d]\n", wkc_, expected_wkc_);
    }
    /* FOR DEBUG END  */
}

void EcatManager::write()
{
    ECAT_CTRL_LOCKER;
    ec_send_processdata();
}

void EcatManager::exit_master()
{
    ec_slave[0].state = EC_STATE_INIT;
    /* request INIT state for all slaves */
    ec_writestate(0);
    fprintf(stderr, "close socket\n");
    ec_close();
}

bool EcatManager::wait_slaves_to_safe_op_()
{
    // 主站等待从站转换到safe-op状态
    /* wait for all slaves to reach SAFE_OP state */
    ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); // 索引0代表所有slave,等待所有slave到达SAFE OP状态
    if (ec_slave[0].state != EC_STATE_SAFE_OP)
    {
        qDebug() << QString("Not all slaves reached safe operational state.\n");
        ec_readstate();
        for (int i = 1; i <= ec_slavecount; i++)
        {
            if (ec_slave[i].state != EC_STATE_SAFE_OP)
            {
                fprintf(stderr, "Slave %d State=%2x StatusCode=%4x : %s\n",
                        i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
            }
        }
        return false;
    }
    else
    {
        return true;
    }
}

bool EcatManager::conf_slaves_to_operational_()
{
    int chk = 40; // 等待从站到operational状态的计数器

    // 主站请求从站转换到Operational状态
    ec_slave[0].state = EC_STATE_OPERATIONAL;
    /* send one valid process data to make outputs in slaves happy*/
    ec_send_processdata();
    ec_receive_processdata(EC_TIMEOUTRET);
    /* request OP state for all slaves */
    ec_writestate(0);

    // 主站等待从站转换到Operational状态,反复尝试请求
    /* wait for all slaves to reach OP state */
    do
    {
        ec_send_processdata();
        ec_receive_processdata(EC_TIMEOUTRET);
        ec_statecheck(0, EC_STATE_OPERATIONAL, 50000);
    } while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));

    // 判断是否到达operational状态
    if (ec_slave[0].state == EC_STATE_OPERATIONAL)
    {
        qDebug() << QString("Operational state reached for all slaves.");

        return true;
    }
    else
    {
        qDebug() << QString("Not all slaves reached operational state.");
        ec_readstate();
        for (int i = 1; i <= ec_slavecount; i++)
        {
            if (ec_slave[i].state != EC_STATE_OPERATIONAL)
            {
                fprintf(stderr, "Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
                        i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
            }
        }

        return false;
    }
}

void EcatManager::conf_servo_pdo_map_()
{
    // 驱动器 PDO 映射
    for (int i = 1; i <= SERVO_NUM; ++i)
    {
        servo_ctrl_[i] = reinterpret_cast<servo_ctrl_t *>(ec_slave[i].outputs);
        //        servo_ctrl_[i] = (servo_ctrl_t *)(ec_slave[i].outputs);
        servo_stat_[i] = reinterpret_cast<servo_stat_t *>(ec_slave[i].inputs);
        //        servo_stat_[i] = (servo_stat_t *)(ec_slave[i].inputs);
    }
}

void EcatManager::conf_servo_default_op_()
{
    for (int i = 1; i <= SERVO_NUM; ++i) {
        servo_ctrl_[i]->modes_of_operation = OM_CSP;
    }
}

void EcatManager::init_servo_position_(int slave_num)
{
    servo_ctrl_[slave_num]->target_position = servo_stat_[slave_num]->position_actual_value;
}
